/*
 * motor_control_core.c
 *
 *  Created on: 2025 Feb 18
 *      Author: SFLY
 */
#include "sensor_motor_control_core.h"


xt_task_motor_RunstatusObser     g_stc_MotorRunStatusObser;//电机运行状态观测器
xt_task_motor_Controller         g_stc_MotorController;//电机控制器
xt_task_motor_Communicator       g_stc_MotorCommunicator;//电机通信控制器

/**************初始化函数**************************/
void Task_Board_Init(void)
{
        //初始化板载驱动
        Bsp_Board_Init();
        //从flash或其他存储设备中读取用户设定默认参数
        Hal_Read_Motor_Param(&g_stc_LogFile);
        //初始化通信控制器
        Task_Motor_Communicator_Init(&g_stc_MotorCommunicator);
        //初始化用户控制器
        Task_Motor_Controller_Init(&g_stc_MotorController);
        //初始化电机运行状态观测器
        Task_Motor_RunStatusObser_Init(&g_stc_MotorRunStatusObser);
}

void Task_Motor_Communicator_Init(xt_task_motor_Communicator *pstc)
{
    pstc->Motor_CanID = g_stc_LogFile.motor_can_id;
    pstc->Master_CanID = g_stc_LogFile.master_can_id;
    Hal_Uart_Communicator_Init(&pstc->uart_commuicator);
    Hal_CanCom_Init( pstc->Motor_CanID,pstc->Master_CanID,XT_HAL_CAN_STD,&pstc->can_commuicator);
}

void Task_Motor_Controller_Init(xt_task_motor_Controller *pstc)
{
   pstc->cur_time = 0;
   pstc->last_time = 0;
   pstc->SoftStartTimestep = 0;
   pstc->target_iq = 0.0f;
   pstc->target_id =  0.0f;
   pstc->target_elec_wr = 0.0f;
   pstc->target_mech_rpm =  0.0f;
   pstc->target_mech_theta =  0.0f;
   pstc->target_mech_aftermotor_theta = 0.0f;
   pstc->target_mech_wr =  0.0f;
   pstc->target_torque = 0.0f;
   float motor_cur_kp = 6.28 * 200.0 * g_stc_LogFile.motor_cur_kp*I_BASE*INV_V_BASE*0.0001;
   float motor_cur_ki =  6.28*200.0*g_stc_LogFile.motor_cur_ki*I_BASE*INV_V_BASE*0.0001;
   float motor_limit_cur_max =g_stc_LogFile.motor_limit_cur_max*INV_I_BASE;
   float motor_limit_cur_min =-g_stc_LogFile.motor_limit_cur_max*INV_I_BASE;
   float motor_limit_cur_err =motor_limit_cur_max*0.8;
   PID_InitIncPid(motor_cur_kp,motor_cur_ki,motor_limit_cur_min,motor_limit_cur_max,motor_limit_cur_err,&pstc->current_q_pid);
   PID_InitIncPid(motor_cur_kp,motor_cur_ki,motor_limit_cur_min,motor_limit_cur_max,motor_limit_cur_err,&pstc->current_d_pid);

//   float motor_speed_kp = g_stc_LogFile.motor_speed_kp*0.0001;//g_stc_LogFile.motor_speed_kp * MATH_TWO_PI * F_BASE*INV_I_BASE*0.0001;
//   float motor_speed_ki = g_stc_LogFile.motor_speed_ki*0.000001;//g_stc_LogFile.motor_speed_ki*0.00001;
//   PID_InitPosPid(motor_speed_kp,motor_speed_ki,motor_limit_cur_min,motor_limit_cur_max,motor_limit_cur_err,&pstc->speed_pid);

//   float motor_pos_kp = g_stc_LogFile.motor_pos_kp*0.01;
//   float motor_limit_spd_max =g_stc_LogFile.motor_limit_spd_max;
//   float motor_limit_spd_min = -g_stc_LogFile.motor_limit_spd_max;
//   float motor_limit_spd_err = g_stc_LogFile.motor_limit_spd_max*0.5;
   //PID_InitPosPid(motor_pos_kp,0,motor_limit_spd_min,motor_limit_spd_max,motor_limit_spd_err,&pstc->pose_pid);
   float motor_pos_kp = g_stc_LogFile.motor_pos_kp*0.00001;
   float motor_pos_ki = g_stc_LogFile.motor_pos_ki*0.0000001;
   float motor_pos_kd = g_stc_LogFile.motor_pos_kd*0.000001;
   PID_InitPosPid(motor_pos_kp,motor_pos_ki,motor_pos_kd,motor_limit_cur_min,motor_limit_cur_max,motor_limit_cur_err,motor_limit_cur_max*0.35,&pstc->pose_pid);
//   PID_InitPosPid(motor_pos_kp,motor_pos_ki,motor_pos_kd,motor_limit_cur_min,motor_limit_cur_max,motor_limit_cur_err,&pstc->pose_pid);

   float motor_pos_torque_kp =g_stc_LogFile.motor_pos_torque_kp*0.0001;
   float motor_pos_torque_ki = g_stc_LogFile.motor_pos_torque_kd*0.0001;
   PID_InitMITPid(motor_pos_torque_kp,motor_pos_torque_ki,&pstc->pos_torque_pid);
}

void Task_Motor_RunStatusObser_Init(xt_task_motor_RunstatusObser *pstc)
{
    pstc->Motor_StdElecAngle = 0.0f;
    pstc->OneMsCnt = 0;
    //初始化电机PWM控制器
    Hal_MotorPwm_Controller_Init(&pstc->Motor_PWM_Controller);
    //初始化电流采样器
    Hal_Current_Sensor_Init(&pstc->Motor_Current_Sensor);
    //初始化电机保护控制器
    Hal_Motor_Protector_Init(&pstc->Motor_Protector);
    //进行一遍保护检测
    pstc->Motor_Protector.motor_vbus_monitor_update(pstc->Motor_Current_Sensor.RVbusObser,&pstc->Motor_Protector);
    pstc->Motor_Protector.motor_current_monitor_update(pstc->Motor_Current_Sensor.IuObser,pstc->Motor_Current_Sensor.IvObser,pstc->Motor_Current_Sensor.IwObser,&pstc->Motor_Protector);
    //初始化电机磁编码器
    if(Task_Motor_Encoder_Init(&pstc->Motor_Encoder))//初始化成功
    {
        //打印欢迎Logo
                printf("/***\n");
                printf(" *       _  __    ______   ______   ____     ______\n");
                printf(" *      | |/ /   /_  __/  / ____/  / __ \\   / ____/\n");
                printf(" *      |   /     / /    / /_     / / / /  / /     \n");
                printf(" *     /   |     / /    / __/    / /_/ /  / /___   \n");
                printf(" *    /_/|_|    /_/    /_/       \\____/   \\____/   \n");
                printf(" *                                                  \n");
                printf(" */\n");
                printf(" Firmware Version: %s\n\r", VERSION_NUM);
                printf(" CAN ID:  %d\n\r", g_stc_MotorCommunicator.Motor_CanID);
                printf(" MASTER CAN ID:  %d\n\r", g_stc_MotorCommunicator.Master_CanID);
                printf(" Motor Voltage: %.2fV\n\r",g_stc_MotorRunStatusObser.Motor_Current_Sensor.RVbusObser);
                printf(" ADC1 Offset:   %.4f    ADC2 Offset:    %.4f\n\r", g_stc_MotorRunStatusObser.Motor_Current_Sensor.IuOffset,g_stc_MotorRunStatusObser.Motor_Current_Sensor.IvOffset);
                printf(" Position Sensor Electrical Offset:   %.4f\n\r", g_stc_MotorRunStatusObser.Motor_Encoder.me_encoder.elec_offset);
                printf(" Mech Zero Position:  %.4f\n\r", g_stc_MotorRunStatusObser.Motor_Encoder.me_encoder.mech_theta_offset);
        pstc->Motor_Status = REST_MODE;
        pstc->Motor_Status_Change = 1;
    }
    else {//初始化失败
        pstc->Motor_Status = CALIBRATION_MODE;//进入校准模式
        pstc->Motor_Status_Change = 1;
    }
    //初始化电机状态
    pstc->Motor_ControlType = Close_Pos_Spd_Current_Controller;//Close_Pos_Spd_Current_Controller;
}


uint8_t Task_Motor_Encoder_Init(xt_task_motor_DoubleEncoder *pstc)
{
   pstc->cur_time = 0;
   pstc->last_time = 0;
   pstc->deltaTimes = 0.0f;
   pstc->Motor_End_Theta_mech =  0.0f;
   pstc->Motor_End_Theta_Filtermech =0.0f;
   pstc->Motor_End_Theta_mech_prev =  0.0f;
   pstc->Reducer_End_Theta_mech = 0.0f;
   pstc->Reducer_End_Theta_Filtermech = 0.0f;
   pstc->Reducer_End_Theta_mech_prev =  0.0f;
   pstc->Reducer_End_RoundCnt =  0.0f;
   pstc->Reducer_End_RoundCnt_prev =  0.0f;
   pstc->Reducer_End_Torque =  0.0f;
   pstc->Reducer_End_mech_Vel_Obser = 0.0f;
   pstc->Motor_End_mech_Vel_Obser = 0.0f;
   pstc->Motor_End_mech_Vel_FilterObser = 0.0f;
   pstc->Motor_End_elec_Vel_Obser = 0.0f;
   pstc->Reducer_End_mech_Vel_RPM_Obser = 0.0f;
   pstc->Motor_End_mech_Vel_RPM_Obser = 0.0f;
   //Filter_InitFirstLpf(SPD_LPF_FC,SPD_LPF_FS,&pstc->velLpf);
   //Q Q R
   EKF_Init(&pstc->PosVelEkf,0.0f,0.0f,1.0f,0.001f,0.0003f,0.0003f,0.001f);
   pstc->isFirstUpdateFlag = 1;
   if(g_stc_LogFile.isUserDefFlag==1)
   {
       Hal_Single_Encoder_Init(&pstc->me_encoder,&pstc->re_encoder);
       if(g_stc_LogFile.signflag&0x0001)//电机端编码器电气偏置为负
       {
           pstc->me_encoder.encoder_set_elec_offset((float)(0-(int)(g_stc_LogFile.motor_elec_offset))*0.0001,&pstc->me_encoder);
       }
       else {
           pstc->me_encoder.encoder_set_elec_offset((float)((int)(g_stc_LogFile.motor_elec_offset))*0.0001,&pstc->me_encoder);
    }
       if(g_stc_LogFile.signflag&0x0002)//电机端编码器机械角度偏置为负
     {
           pstc->me_encoder.encoder_set_mech_offset((float)(0-(int)(g_stc_LogFile.motor_mech_offset))*0.0001,&pstc->me_encoder);
     }
       else{
           pstc->me_encoder.encoder_set_mech_offset((float)((int)(g_stc_LogFile.motor_mech_offset))*0.0001,&pstc->me_encoder);
       }
       if(g_stc_LogFile.signflag&0x0004)//电机端编码器相序为负
     {
           pstc->me_encoder.encoder_set_order(g_stc_LogFile.motor_orderdir,&pstc->me_encoder);
     }
       else{
           pstc->me_encoder.encoder_set_order(g_stc_LogFile.motor_orderdir,&pstc->me_encoder);
       }
       if(g_stc_LogFile.signflag&0x0008)//减速器端编码器机械角度偏置为负
     {
           pstc->re_encoder.encoder_set_mech_offset((float)(0-(int)(g_stc_LogFile.reducer_mech_offset))*0.0001,&pstc->re_encoder);
     }
       else{
           pstc->re_encoder.encoder_set_mech_offset((float)((int)(g_stc_LogFile.reducer_mech_offset))*0.0001,&pstc->re_encoder);
       }
       if(g_stc_LogFile.signflag&0x0010)//减速器端编码器相序为负
      {
           pstc->re_encoder.encoder_set_order(g_stc_LogFile.reducer_orderdir,&pstc->re_encoder);
      }
        else{
            pstc->re_encoder.encoder_set_order(g_stc_LogFile.reducer_orderdir,&pstc->re_encoder);
        }
       memcpy(&pstc->me_encoder.offset_lut, g_stc_LogFile.motor_offset_lut, sizeof(g_stc_LogFile.motor_offset_lut));
       return 1;
   }
   else//若没有校准，则第一次初始化的时候需要校准
   {
       Hal_Single_Encoder_Init(&pstc->me_encoder,&pstc->re_encoder);
       pstc->me_encoder.encoder_set_order(1,&pstc->me_encoder);
       pstc->re_encoder.encoder_set_order(1,&pstc->re_encoder);
       return 0;
   }
}

/**************************************************************************************************************/
void Task_Motor_Elec_Update(xt_task_motor_RunstatusObser *motorobser)
{
       //1.读取原始数据
       motorobser->Motor_Encoder.me_encoder.encoder_update(&motorobser->Motor_Encoder.me_encoder);
       motorobser->Motor_Encoder.Motor_End_Theta_elec  = motorobser->Motor_Encoder.me_encoder.elec_theta*INV_MATH_TWO_PI;
}

void Task_Motor_Encoder_Update(xt_task_motor_DoubleEncoder *motorencoder)
{
    //2.计算电机端角度数据
    motorencoder->re_encoder.encoder_update(&motorencoder->re_encoder);
    motorencoder->Motor_End_Theta_mech =  motorencoder->me_encoder.multi_mech_theta;
    motorencoder->Reducer_End_Theta_mech = motorencoder->Motor_End_Theta_mech+motorencoder->re_encoder.multi_mech_theta;
    motorencoder->cur_time = Bsp_Get_Timer_Count();
    //3.计算电机端机械角速度
    if(motorencoder->isFirstUpdateFlag==1)
    {
        motorencoder->isFirstUpdateFlag = 0;
        motorencoder->Motor_End_Theta_Filtermech =  0.0f;
        motorencoder->Motor_End_Theta_Filtermech = motorencoder->PosVelEkf.theta;
        motorencoder->Reducer_End_Theta_Filtermech =  motorencoder->Reducer_End_Theta_mech;
        motorencoder->Motor_End_mech_Vel_FilterObser =  0.0f;
        motorencoder->Motor_End_mech_Vel_RPM_Obser = 0.0f;
        motorencoder->last_time =  motorencoder->cur_time;
    }
    else
    {
        motorencoder->deltaTimes =  Bsp_Calculate_Delta_Count(motorencoder->cur_time, motorencoder->last_time);
        EKF_Predict(&motorencoder->PosVelEkf, motorencoder->deltaTimes);
        EKF_Update(&motorencoder->PosVelEkf,motorencoder->Reducer_End_Theta_mech);
        motorencoder->Reducer_End_Theta_Filtermech = motorencoder->PosVelEkf.theta;
        //motorencoder->Reducer_End_Theta_mech = motorencoder->Motor_End_Theta_Filtermech+motorencoder->re_encoder.multi_mech_theta;
        motorencoder->Motor_End_mech_Vel_FilterObser = motorencoder->PosVelEkf.omega;//Filter_FirstLpf(motorencoder->Motor_End_mech_Vel_Obser,&motorencoder->velLpf);
        motorencoder->Motor_End_mech_Vel_RPM_Obser = motorencoder->Motor_End_mech_Vel_FilterObser*30.0*INV_MATH_PI;
        motorencoder->last_time =  motorencoder->cur_time;
    }
}


void Task_Time1msLoopEvent(xt_task_motor_RunstatusObser *motorobser, xt_task_motor_Controller *controller)
{
        motorobser->OneMsCnt++;
        if( motorobser->OneMsCnt%1==0)
        {
            motorobser->Motor_Protector.motor_current_monitor_update(motorobser->Motor_Current_Sensor.IuObser,motorobser->Motor_Current_Sensor.IvObser,motorobser->Motor_Current_Sensor.IwObser,&motorobser->Motor_Protector);
            if(motorobser->Motor_Protector.ErrorFlag!=ERR_NONE)
              {
                Task_Motor_FastStop(motorobser,controller);
                printf("Over Current Err!!!!!\r\n");
              }
        }
        if(motorobser->OneMsCnt%10==0)
        {
            motorobser->OneMsCnt = 0;
            if(motorobser->Motor_Status==MOTOR_MODE)
                       {
                           //1.获取电机角度和角速度
                           Task_Motor_Encoder_Update(&motorobser->Motor_Encoder);
                           switch(motorobser->Motor_ControlType)
                           {
                           case Open_Controller:
                           {
                           }
                               break;
                           case Close_Pos_Spd_Current_Controller:
                           {
                                 controller->pose_pid.Kp = g_stc_LogFile.motor_pos_kp*0.00001;
                                 controller->pose_pid.Ki = g_stc_LogFile.motor_pos_ki*0.0000001;
                                 controller->pose_pid.Kd = g_stc_LogFile.motor_pos_kd*0.000001;
                                 //controller->target_mech_aftermotor_theta = controller->target_mech_theta*9.0f;
                                 controller->target_iq = PID_PosPid(controller->target_mech_theta ,motorobser->Motor_Encoder.Reducer_End_Theta_Filtermech, &controller->pose_pid);
                                 controller->target_id = 0;
                                 printf("%f,%f\n",controller->target_mech_theta,motorobser->Motor_Encoder.Reducer_End_Theta_Filtermech);
                           }
                               break;
                           case Close_Pos_Spd_Torque_Current_Controller:
                           {
                               controller->pos_torque_pid.pos_Kp =g_stc_LogFile.motor_pos_torque_kp*0.0001;
                               controller->pos_torque_pid.spd_Kd = g_stc_LogFile.motor_pos_torque_kd*0.0001;
                               controller->target_mech_aftermotor_theta = controller->target_mech_theta*(float)(MOTOR_ReductionRatio);
                               controller->target_mech_rpms = controller->target_mech_rpm*(float)(MOTOR_ReductionRatio);
                               controller->target_iq  = PID_MITPid(controller->target_mech_aftermotor_theta,motorobser->Motor_Encoder.Reducer_End_Theta_Filtermech,controller->target_mech_rpms,motorobser->Motor_Encoder.Motor_End_mech_Vel_RPM_Obser,controller->target_torque,&controller->pos_torque_pid);
                               controller->target_id = 0;
                           }
                               break;
                           default:
                               break;
                           }
               }
        }
}




/**********主循环事件***************/
//位置开环-速度闭环-电流闭环
void Task_Motor_PosSpdLoopCurrentLoopRun(xt_task_motor_RunstatusObser *motorobser, xt_task_motor_Controller *controller)
{
        motorobser->Motor_Current_Sensor.current_sensor_update(&motorobser->Motor_Current_Sensor);
        motorobser->Motor_IuvwObser.Ia = motorobser->Motor_Current_Sensor.IuObser;
        motorobser->Motor_IuvwObser.Ib = motorobser->Motor_Current_Sensor.IvObser;
        motorobser->Motor_IuvwObser.Ic = motorobser->Motor_Current_Sensor.IwObser;
         //2.获得观测的电角度
         Task_Motor_Elec_Update(motorobser);
         //手动加电角度
         motorobser->Motor_StdElecAngle = motorobser->Motor_Encoder.Motor_End_Theta_elec;
           //3.计算观测到的q轴和d轴电流
          Math_SinCos(motorobser->Motor_StdElecAngle, & motorobser->Motor_IdqObser.sin,  & motorobser->Motor_IdqObser.cos);
          Transf_Clarke(motorobser->Motor_IuvwObser, &motorobser->Motor_IalphaBetaObser);
          Transf_Park(motorobser->Motor_IalphaBetaObser, &motorobser->Motor_IdqObser);
          //4.计算用户期望的q轴和d轴电流
          motorobser->Motor_IdqRef.Iq = controller->target_iq;
          motorobser->Motor_IdqRef.Id = controller->target_id;
          motorobser->Motor_IdqRef.sin = motorobser->Motor_IdqObser.sin;
          motorobser->Motor_IdqRef.cos = motorobser->Motor_IdqObser.cos;
          motorobser->Motor_IdqRef.Id = PID_IncPid(motorobser->Motor_IdqRef.Id, motorobser->Motor_IdqObser.Id, &g_stc_MotorController.current_d_pid);
          motorobser->Motor_IdqRef.Iq = PID_IncPid(motorobser->Motor_IdqRef.Iq, motorobser->Motor_IdqObser.Iq, &g_stc_MotorController.current_q_pid);
          Transf_InvPark(motorobser->Motor_IdqRef, &motorobser->Motor_IalphaBetaRef);
          //2.SVPWM
          /** PWM calculation, and generate switch pattern of SVPWM *********/
          motorobser->Motor_PWM_Controller.svpwm_caculate(motorobser->Motor_IalphaBetaRef.Ialpha,motorobser->Motor_IalphaBetaRef.Ibeta,motorobser->Motor_Current_Sensor.StdVbusObser,&motorobser->Motor_PWM_Controller.svpwm_caculator);
          motorobser->Motor_PWM_Controller.motor_pwm_caculate(&motorobser->Motor_PWM_Controller.svpwm_caculator,&motorobser->Motor_PWM_Controller);
          motorobser->Motor_PWM_Controller.motor_pwm_set(motorobser->Motor_PWM_Controller.u16Uon,motorobser->Motor_PWM_Controller.u16Von,motorobser->Motor_PWM_Controller.u16Won);
}

//位置开环-速度开环-电流闭环
float delta_spd = 0.0;
void Task_Motor_PosSpdCurrentLoopRun(xt_task_motor_RunstatusObser *motorobser, xt_task_motor_Controller *controller)
{
        motorobser->Motor_Current_Sensor.current_sensor_update(&motorobser->Motor_Current_Sensor);
        motorobser->Motor_IuvwObser.Ia = motorobser->Motor_Current_Sensor.IuObser;
        motorobser->Motor_IuvwObser.Ib = motorobser->Motor_Current_Sensor.IvObser;
        motorobser->Motor_IuvwObser.Ic = motorobser->Motor_Current_Sensor.IwObser;
         //2.获得观测的电角度
         Task_Motor_Elec_Update(motorobser);
         //手动加电角度
         motorobser->Motor_StdElecAngle = motorobser->Motor_StdElecAngle+delta_spd;
         // motorstatus->Motor_StdElecAngle = motorstatus->Motor_Encoder.Motor_End_Theta_elec;
         if(motorobser->Motor_StdElecAngle > 1.0f) {
             motorobser->Motor_StdElecAngle -= 1.0f;
           } else if(motorobser->Motor_StdElecAngle < 0.0f) {
               motorobser->Motor_StdElecAngle += 1.0f;
           }
           //3.计算观测到的q轴和d轴电流
          Math_SinCos(motorobser->Motor_StdElecAngle, & motorobser->Motor_IdqObser.sin,  & motorobser->Motor_IdqObser.cos);
          Transf_Clarke(motorobser->Motor_IuvwObser, &motorobser->Motor_IalphaBetaObser);
          Transf_Park(motorobser->Motor_IalphaBetaObser, &motorobser->Motor_IdqObser);
          //4.计算用户期望的q轴和d轴电流
          motorobser->Motor_IdqRef.Iq = controller->target_iq;
          motorobser->Motor_IdqRef.Id = controller->target_id;
          motorobser->Motor_IdqRef.sin = motorobser->Motor_IdqObser.sin;
          motorobser->Motor_IdqRef.cos = motorobser->Motor_IdqObser.cos;
          motorobser->Motor_IdqRef.Id = PID_IncPid(motorobser->Motor_IdqRef.Id, motorobser->Motor_IdqObser.Id, &g_stc_MotorController.current_d_pid);
          motorobser->Motor_IdqRef.Iq = PID_IncPid(motorobser->Motor_IdqRef.Iq, motorobser->Motor_IdqObser.Iq, &g_stc_MotorController.current_q_pid);
          Transf_InvPark(motorobser->Motor_IdqRef, &motorobser->Motor_IalphaBetaRef);
          //2.SVPWM
          /** PWM calculation, and generate switch pattern of SVPWM *********/
          motorobser->Motor_PWM_Controller.svpwm_caculate(motorobser->Motor_IalphaBetaRef.Ialpha,motorobser->Motor_IalphaBetaRef.Ibeta,motorobser->Motor_Current_Sensor.StdVbusObser,&motorobser->Motor_PWM_Controller.svpwm_caculator);
          motorobser->Motor_PWM_Controller.motor_pwm_caculate(&motorobser->Motor_PWM_Controller.svpwm_caculator,&motorobser->Motor_PWM_Controller);
          motorobser->Motor_PWM_Controller.motor_pwm_set(motorobser->Motor_PWM_Controller.u16Uon,motorobser->Motor_PWM_Controller.u16Von,motorobser->Motor_PWM_Controller.u16Won);
}

//1.判断是否是开环控制
//2.如果是闭环控制，需要判断编码器和ADC是否ready
//3.如果编码器没有ready，则提醒用户需要进行编码器零位校准，否则不能进入闭环控制
//4.进入闭环后需要判断是闭环的模式：要根据用户的选择自由组合模式（I、SI、PI、PS、PSI）
void Task_MainLoopEvent(xt_task_motor_RunstatusObser *motorobser, xt_task_motor_Controller *controller,xt_task_motor_Communicator *motorcommunitor)
{
    //last_time = Bsp_Get_Timer_Count();
       switch(motorobser->Motor_Status)
               {
               case MOTOR_MODE://电机运行状态
               {
                 if(motorobser->Motor_Status_Change)
                   {
                     //采样静止电流
                     if(FALSE==motorobser->Motor_Current_Sensor.CurrentSensorRdy)
                     {
                         motorobser->Motor_Current_Sensor.current_sensor_checkoffset(&motorobser->Motor_Current_Sensor);
                     }
                     else {
                         //电机闭环前的初始化
                         controller->target_iq = 0.0f;
                         controller->target_id = 0.0f;
                         controller->target_mech_rpm = 0.0f;
                         controller->target_mech_theta = 0.0f;
                         motorobser->Motor_PWM_Controller.motor_pwm_enable(TRUE);
                         motorobser->Motor_Status_Change = 0;
                   }
                   }
                 else {
                    //Task_Motor_PosSpdCurrentLoopRun(motorobser,controller);
                     Task_Motor_PosSpdLoopCurrentLoopRun(motorobser,controller);
               }
                }
               break;
               case CALIBRATION_MODE://编码器标定状态
               {
                 if(motorobser->Motor_Status_Change)
                 {
                     Task_Encoder_Calibration(motorobser,controller,motorcommunitor);
                 }
               }
                 break;
               case REST_MODE: //什么都不做状态
                 if(motorobser->Motor_Status_Change)
                  {
                       Task_Motor_FastStop(motorobser,controller);
                       printf("\n\r\n\r\n\r");
                       printf(" Commands:\n\r");
                       printf(" m - Motor Mode\n\r");
                       printf(" c - Calibrate Encoder\n\r");
                       printf(" s - Setup\n\r");
                       printf(" e - Display Encoder\n\r");
                       printf(" z - Set Zero Position\n\r");
                       printf(" esc - Exit to Menu\n\r");
                       motorobser->Motor_Status_Change = 0;
                  }
                 break;
               case SETUP_MODE:
                   if(motorobser->Motor_Status_Change){
                       printf("\n\r\n\r Configuration Options \n\r\n\n");
                       printf(" %-4s %-31s %-5s %-6s %-2s\n\r\n\r", "prefix", "parameter", "min", "max", "current value");
                       printf(" %-4s %-31s %-5s %-6s %-5i\n\r", "i", "CAN ID", "0", "127", g_stc_MotorCommunicator.Motor_CanID);
                       printf(" %-4s %-31s %-5s %-6s %-5i\n\r", "m", "CAN Master ID", "0", "127", g_stc_MotorCommunicator.Master_CanID);
                       printf("\n\r To change a value, type 'prefix''value''ENTER'\n\r i.e. 'i1''ENTER'\n\r\n\r");
                       motorobser->Motor_Status_Change = 0;
                   }
                   break;
               case ENCODER_MODE:
                       Task_Motor_Elec_Update(motorobser);
                       Task_Motor_Encoder_Update(&motorobser->Motor_Encoder);
                       printf("%f,%f\n",g_stc_MotorRunStatusObser.Motor_Encoder.Reducer_End_Theta_mech,g_stc_MotorRunStatusObser.Motor_Encoder.Reducer_End_Theta_Filtermech);
                       //printf("Current Mechanical Angle: %f  Current Reducer Angle: %f\r\n",g_stc_MotorRunStatusObser.Motor_Encoder.Motor_End_Theta_Filtermech,g_stc_MotorRunStatusObser.Motor_Encoder.Reducer_End_Theta_Filtermech);
               default:
                 break;
               }
}




void Task_Motor_FastStop(xt_task_motor_RunstatusObser *motorobser, xt_task_motor_Controller *controller)
{
    //失能EPWM
    motorobser->Motor_PWM_Controller.motor_pwm_enable(FALSE);
    motorobser->Motor_StdElecAngle = 0.0f;
    motorobser->Motor_IdqRef.Iq = 0.0f;
    motorobser->Motor_IdqRef.Id = 0.0f;
    motorobser->Motor_IdqObser.Iq = 0.0f;
    motorobser->Motor_IdqObser.Id = 0.0f;
    motorobser->Motor_IalphaBetaRef.Ialpha = 0.0f;
    motorobser->Motor_IalphaBetaRef.Ibeta = 0.0f;
    controller->target_iq = 0.0f;
    controller->target_id = 0.0f;
    controller->target_mech_rpm = 0.0f;
    controller->target_mech_theta = 0.0f;
    controller->target_mech_aftermotor_theta = 0.0f;
    controller->target_mech_wr = 0.0f;
    controller->target_torque = 0.0f;
    controller->pose_pid.Iout = 0.0f;
    controller->pose_pid.Output = 0.0f;
    controller->pose_pid.Err1  = 0.0f;
    controller->pose_pid.Err2 = 0.0f;

    /***************************************************************************
        ** clear SVPWM calculation and PWM generation parameters
        **************************************************************************/
    motorobser->Motor_PWM_Controller.motor_pwm_reset(&motorobser->Motor_PWM_Controller);
}


/*********************************************通信协议*****************************************************/
//串口协议
__interrupt void scibISR(void)
{
    g_stc_MotorCommunicator.uart_commuicator.recvCurCmd = SCI_readCharNonBlocking(SCIB_BASE);
    //首先判断是不是ESC，如果是ESC则进入REST_MODE
    if(g_stc_MotorCommunicator.uart_commuicator.recvCurCmd ==0x1B)
    {
        g_stc_MotorRunStatusObser.Motor_Status = REST_MODE;
        g_stc_MotorRunStatusObser.Motor_Status_Change = 1;
        g_stc_MotorCommunicator.uart_commuicator.char_count = 0;
        g_stc_MotorCommunicator.uart_commuicator.cmd_id =0;
        for(int i=0;i<8;i++)
        {
            g_stc_MotorCommunicator.uart_commuicator.recvValueCmd[i] = 0;
        }
    }
    if(g_stc_MotorRunStatusObser.Motor_Status==REST_MODE)
    {
        switch (g_stc_MotorCommunicator.uart_commuicator.recvCurCmd ){
               case 'c':
                   g_stc_MotorRunStatusObser.Motor_Status = CALIBRATION_MODE;
                   g_stc_MotorRunStatusObser.Motor_Status_Change = 1;
                   break;
               case 'm':
                   g_stc_MotorRunStatusObser.Motor_Status = MOTOR_MODE;
                   g_stc_MotorRunStatusObser.Motor_Status_Change = 1;
                   break;
               case 'e':
                   g_stc_MotorRunStatusObser.Motor_Status = ENCODER_MODE;
                   g_stc_MotorRunStatusObser.Motor_Status_Change = 1;
                   break;
               case 's':
                   g_stc_MotorRunStatusObser.Motor_Status = SETUP_MODE;
                   g_stc_MotorRunStatusObser.Motor_Status_Change = 1;
                   break;
               case 'z':
                   g_stc_MotorRunStatusObser.Motor_Encoder.me_encoder.encoder_set_mech_offset(0,&g_stc_MotorRunStatusObser.Motor_Encoder.me_encoder);
                   g_stc_MotorRunStatusObser.Motor_Encoder.re_encoder.encoder_set_mech_offset(0,&g_stc_MotorRunStatusObser.Motor_Encoder.re_encoder);
                   g_stc_MotorRunStatusObser.Motor_Encoder.me_encoder.encoder_update(&g_stc_MotorRunStatusObser.Motor_Encoder.me_encoder);
                   g_stc_MotorRunStatusObser.Motor_Encoder.me_encoder.encoder_set_mech_offset(g_stc_MotorRunStatusObser.Motor_Encoder.me_encoder.abs_multi_mech_theta,&g_stc_MotorRunStatusObser.Motor_Encoder.me_encoder);
                   g_stc_MotorRunStatusObser.Motor_Encoder.re_encoder.encoder_update(&g_stc_MotorRunStatusObser.Motor_Encoder.re_encoder);
                   g_stc_MotorRunStatusObser.Motor_Encoder.re_encoder.encoder_set_mech_offset(g_stc_MotorRunStatusObser.Motor_Encoder.re_encoder.abs_multi_mech_theta,&g_stc_MotorRunStatusObser.Motor_Encoder.re_encoder);
                   Task_Encoder_SavedCalibrationResult(&g_stc_MotorRunStatusObser,&g_stc_MotorCommunicator);
                   printf("\n\r  Saved new zero position:  %.4f\n\r\n\r", g_stc_MotorRunStatusObser.Motor_Encoder.me_encoder.mech_theta_offset);
                   break;
               }
    }
    else if(g_stc_MotorRunStatusObser.Motor_Status == SETUP_MODE){
        if(g_stc_MotorCommunicator.uart_commuicator.recvCurCmd == 0x27){
              switch (g_stc_MotorCommunicator.uart_commuicator.cmd_id){
                      break;
                  case 'i'://修改MOTOR CAN ID
                        g_stc_MotorCommunicator.Motor_CanID = atoi(g_stc_MotorCommunicator.uart_commuicator.recvValueCmd);
                        g_stc_MotorCommunicator.can_commuicator.can_id = g_stc_MotorCommunicator.Motor_CanID;
                        printf("Set Motor CAN ID is %d\r\n",g_stc_MotorCommunicator.Motor_CanID);
                        break;
                  case 'm'://修改MASTER CAN ID
                        g_stc_MotorCommunicator.Master_CanID = atoi(g_stc_MotorCommunicator.uart_commuicator.recvValueCmd);
                        g_stc_MotorCommunicator.can_commuicator.master_can_id = g_stc_MotorCommunicator.Master_CanID;
                        printf("Set Master CAN ID is %d\r\n",g_stc_MotorCommunicator.Master_CanID);
                        break;
                  default:
                      printf("\n\r '%c' Not a valid command prefix\n\r\n\r",  g_stc_MotorCommunicator.uart_commuicator.cmd_id);
                      break;
                  }
              g_stc_MotorRunStatusObser.Motor_Status_Change = 1;
              g_stc_MotorCommunicator.uart_commuicator.char_count = 0;
              g_stc_MotorCommunicator.uart_commuicator.cmd_id = 0;
              //保存更新后的数据
              Task_Encoder_SavedCalibrationResult(&g_stc_MotorRunStatusObser,&g_stc_MotorCommunicator);
              for(int i = 0; i<8; i++){g_stc_MotorCommunicator.uart_commuicator.recvValueCmd[i] = 0;}
              }
          else{
              if( g_stc_MotorCommunicator.uart_commuicator.char_count == 0){ g_stc_MotorCommunicator.uart_commuicator.cmd_id = g_stc_MotorCommunicator.uart_commuicator.recvCurCmd ;}
              else{
                  g_stc_MotorCommunicator.uart_commuicator.recvValueCmd[ g_stc_MotorCommunicator.uart_commuicator.char_count-1] = g_stc_MotorCommunicator.uart_commuicator.recvCurCmd ;
              }
              g_stc_MotorCommunicator.uart_commuicator.char_count++;
              }
    }
    else if (g_stc_MotorRunStatusObser.Motor_Status == ENCODER_MODE){

        }
    else if (g_stc_MotorRunStatusObser.Motor_Status == MOTOR_MODE){
        if(g_stc_MotorCommunicator.uart_commuicator.recvCurCmd == 0x27){
                  switch (g_stc_MotorCommunicator.uart_commuicator.cmd_id){
                          break;
                      case 'p'://设置期望角度
                            g_stc_MotorController.target_mech_theta = (float)(atoi(g_stc_MotorCommunicator.uart_commuicator.recvValueCmd))*ANGLE2RAD;
                            printf("Set Motor Target Theta is %f\r\n", g_stc_MotorController.target_mech_theta);
                            break;
                      case 's'://设置期望速度

                            break;
                      default:
                          printf("\n\r '%c' Not a valid command prefix\n\r\n\r",  g_stc_MotorCommunicator.uart_commuicator.cmd_id);
                          break;
                      }
                  g_stc_MotorCommunicator.uart_commuicator.char_count = 0;
                  g_stc_MotorCommunicator.uart_commuicator.cmd_id = 0;
                  for(int i = 0; i<8; i++){g_stc_MotorCommunicator.uart_commuicator.recvValueCmd[i] = 0;}
                  }
              else{
                  if( g_stc_MotorCommunicator.uart_commuicator.char_count == 0){ g_stc_MotorCommunicator.uart_commuicator.cmd_id = g_stc_MotorCommunicator.uart_commuicator.recvCurCmd ;}
                  else{
                      g_stc_MotorCommunicator.uart_commuicator.recvValueCmd[ g_stc_MotorCommunicator.uart_commuicator.char_count-1] = g_stc_MotorCommunicator.uart_commuicator.recvCurCmd ;
                  }
                  g_stc_MotorCommunicator.uart_commuicator.char_count++;
                  }
    }
    SCI_clearInterruptStatus(SCIB_BASE, SCI_INT_RXRDY);
}

//can协议
__interrupt void canfdISR(void)
{
        if (CAN_getStatus(CANFDB_BASE, CAN_FLAG_RX))
        {
            Hal_CanCom_RecvParse(&g_stc_MotorCommunicator.can_commuicator);
            if(g_stc_MotorCommunicator.can_commuicator.isRecvFlag ==1)
            {
                g_stc_MotorCommunicator.can_commuicator.isRecvFlag= 0;
                           if((g_stc_MotorCommunicator.can_commuicator.recv_candata[0]==0xFF)&
                              (g_stc_MotorCommunicator.can_commuicator.recv_candata[1]==0xFF)&
                              (g_stc_MotorCommunicator.can_commuicator.recv_candata[2]==0xFF)&
                              (g_stc_MotorCommunicator.can_commuicator.recv_candata[3]==0xFF)&
                              (g_stc_MotorCommunicator.can_commuicator.recv_candata[4]==0xFF)&
                              (g_stc_MotorCommunicator.can_commuicator.recv_candata[5]==0xFF)&
                              (g_stc_MotorCommunicator.can_commuicator.recv_candata[6]==0xFF)&
                              (g_stc_MotorCommunicator.can_commuicator.recv_candata[7]==0xFA))
                           {
                               g_stc_MotorRunStatusObser.Motor_Status = REST_MODE;
                               g_stc_MotorRunStatusObser.Motor_Status_Change = 1;
                           }
                       if(g_stc_MotorRunStatusObser.Motor_Status==REST_MODE)
                         {
                               if((g_stc_MotorCommunicator.can_commuicator.recv_candata[0]==0xFF)&
                                  (g_stc_MotorCommunicator.can_commuicator.recv_candata[1]==0xFF)&
                                  (g_stc_MotorCommunicator.can_commuicator.recv_candata[2]==0xFF)&
                                  (g_stc_MotorCommunicator.can_commuicator.recv_candata[3]==0xFF)&
                                  (g_stc_MotorCommunicator.can_commuicator.recv_candata[4]==0xFF)&
                                  (g_stc_MotorCommunicator.can_commuicator.recv_candata[5]==0xFF)&
                                  (g_stc_MotorCommunicator.can_commuicator.recv_candata[6]==0xFF))
                               {
                                 switch(g_stc_MotorCommunicator.can_commuicator.recv_candata[7])
                                 {
                                       case 0xFB://校准模式
                                       g_stc_MotorRunStatusObser.Motor_Status = CALIBRATION_MODE;
                                       g_stc_MotorRunStatusObser.Motor_Status_Change = 1;
                                       break;
                                       case 0xFC://电机运行模式
                                       g_stc_MotorRunStatusObser.Motor_Status = MOTOR_MODE;
                                       g_stc_MotorRunStatusObser.Motor_Status_Change = 1;
                                       break;
                                       case 0xFD://设置模式
                                       g_stc_MotorRunStatusObser.Motor_Status = SETUP_MODE;
                                       g_stc_MotorRunStatusObser.Motor_Status_Change = 1;
                                       break;
                                       case 0xFE://编码器模式
                                       g_stc_MotorRunStatusObser.Motor_Status = ENCODER_MODE;
                                       g_stc_MotorRunStatusObser.Motor_Status_Change = 1;
                                       break;
                                       case 0xFF://设置零点
                                       g_stc_MotorRunStatusObser.Motor_Encoder.me_encoder.encoder_set_mech_offset(0,&g_stc_MotorRunStatusObser.Motor_Encoder.me_encoder);
                                       g_stc_MotorRunStatusObser.Motor_Encoder.re_encoder.encoder_set_mech_offset(0,&g_stc_MotorRunStatusObser.Motor_Encoder.re_encoder);
                                       g_stc_MotorRunStatusObser.Motor_Encoder.me_encoder.encoder_update(&g_stc_MotorRunStatusObser.Motor_Encoder.me_encoder);
                                       g_stc_MotorRunStatusObser.Motor_Encoder.me_encoder.encoder_set_mech_offset(g_stc_MotorRunStatusObser.Motor_Encoder.me_encoder.abs_multi_mech_theta,&g_stc_MotorRunStatusObser.Motor_Encoder.me_encoder);
                                       g_stc_MotorRunStatusObser.Motor_Encoder.re_encoder.encoder_update(&g_stc_MotorRunStatusObser.Motor_Encoder.re_encoder);
                                       g_stc_MotorRunStatusObser.Motor_Encoder.re_encoder.encoder_set_mech_offset(g_stc_MotorRunStatusObser.Motor_Encoder.re_encoder.abs_multi_mech_theta,&g_stc_MotorRunStatusObser.Motor_Encoder.re_encoder);
                                       Task_Encoder_SavedCalibrationResult(&g_stc_MotorRunStatusObser,&g_stc_MotorCommunicator);
                                     break;
                                 }
                             }
                          }
                       else if(g_stc_MotorRunStatusObser.Motor_Status == SETUP_MODE)
                       {
                           if(g_stc_MotorCommunicator.can_commuicator.recv_candata[0]==0xFF)
                           {
                               switch(g_stc_MotorCommunicator.can_commuicator.recv_candata[1])
                               {
                               case 0x01:
                                    g_stc_MotorCommunicator.Motor_CanID = g_stc_MotorCommunicator.can_commuicator.recv_candata[2] << 8 | g_stc_MotorCommunicator.can_commuicator.recv_candata[3];
                                    g_stc_MotorCommunicator.can_commuicator.can_id = g_stc_MotorCommunicator.Motor_CanID;
                                    g_stc_MotorCommunicator.Master_CanID = g_stc_MotorCommunicator.can_commuicator.recv_candata[4] << 8 | g_stc_MotorCommunicator.can_commuicator.recv_candata[5];
                                    g_stc_MotorCommunicator.can_commuicator.master_can_id = g_stc_MotorCommunicator.Master_CanID;
                                   break;
                               case 0x02:
                                   g_stc_LogFile.motor_cur_kp = ((uint32_t)g_stc_MotorCommunicator.can_commuicator.recv_candata[2] << 24) | ((uint32_t)g_stc_MotorCommunicator.can_commuicator.recv_candata[3] << 16) | ((uint32_t)g_stc_MotorCommunicator.can_commuicator.recv_candata[4] << 8) | g_stc_MotorCommunicator.can_commuicator.recv_candata[5];
                                   break;
                               case 0x03:
                                   g_stc_LogFile.motor_cur_ki = ((uint32_t)g_stc_MotorCommunicator.can_commuicator.recv_candata[2] << 24) | ((uint32_t)g_stc_MotorCommunicator.can_commuicator.recv_candata[3] << 16) | ((uint32_t)g_stc_MotorCommunicator.can_commuicator.recv_candata[4] << 8) | g_stc_MotorCommunicator.can_commuicator.recv_candata[5];
                                   break;
                               case 0x04:
                                   g_stc_LogFile.motor_speed_kp = ((uint32_t)g_stc_MotorCommunicator.can_commuicator.recv_candata[2] << 24) | ((uint32_t)g_stc_MotorCommunicator.can_commuicator.recv_candata[3] << 16) | ((uint32_t)g_stc_MotorCommunicator.can_commuicator.recv_candata[4] << 8) | g_stc_MotorCommunicator.can_commuicator.recv_candata[5];
                                   break;
                               case 0x05:
                                   g_stc_LogFile.motor_speed_ki = ((uint32_t)g_stc_MotorCommunicator.can_commuicator.recv_candata[2] << 24) | ((uint32_t)g_stc_MotorCommunicator.can_commuicator.recv_candata[3] << 16) | ((uint32_t)g_stc_MotorCommunicator.can_commuicator.recv_candata[4] << 8) | g_stc_MotorCommunicator.can_commuicator.recv_candata[5];
                                   break;
                               case 0x06:
                                   g_stc_LogFile.motor_pos_kp = ((uint32_t)g_stc_MotorCommunicator.can_commuicator.recv_candata[2] << 24) | ((uint32_t)g_stc_MotorCommunicator.can_commuicator.recv_candata[3] << 16) | ((uint32_t)g_stc_MotorCommunicator.can_commuicator.recv_candata[4] << 8) | g_stc_MotorCommunicator.can_commuicator.recv_candata[5];
                                   break;
                               case 0x07:
                                   g_stc_LogFile.motor_pos_ki = ((uint32_t)g_stc_MotorCommunicator.can_commuicator.recv_candata[2] << 24) | ((uint32_t)g_stc_MotorCommunicator.can_commuicator.recv_candata[3] << 16) | ((uint32_t)g_stc_MotorCommunicator.can_commuicator.recv_candata[4] << 8) | g_stc_MotorCommunicator.can_commuicator.recv_candata[5];
                                   break;
                               case 0x08:
                                   g_stc_LogFile.motor_pos_kd = ((uint32_t)g_stc_MotorCommunicator.can_commuicator.recv_candata[2] << 24) | ((uint32_t)g_stc_MotorCommunicator.can_commuicator.recv_candata[3] << 16) | ((uint32_t)g_stc_MotorCommunicator.can_commuicator.recv_candata[4] << 8) | g_stc_MotorCommunicator.can_commuicator.recv_candata[5];
                                   break;
                               case 0x09:
                                   g_stc_LogFile.motor_limit_cur_max = ((uint32_t)g_stc_MotorCommunicator.can_commuicator.recv_candata[2] << 24) | ((uint32_t)g_stc_MotorCommunicator.can_commuicator.recv_candata[3] << 16) | ((uint32_t)g_stc_MotorCommunicator.can_commuicator.recv_candata[4] << 8) | g_stc_MotorCommunicator.can_commuicator.recv_candata[5];
                                   break;
                               case 0x0A:
                                   g_stc_LogFile.motor_limit_spd_max = ((uint32_t)g_stc_MotorCommunicator.can_commuicator.recv_candata[2] << 24) | ((uint32_t)g_stc_MotorCommunicator.can_commuicator.recv_candata[3] << 16) | ((uint32_t)g_stc_MotorCommunicator.can_commuicator.recv_candata[4] << 8) | g_stc_MotorCommunicator.can_commuicator.recv_candata[5];
                                   break;
                               case 0x0B:
                                   break;
                               case 0x0C:
                                   break;
                               case 0x0D:
                                   break;
                               case 0x0E:
                                   break;
                               case 0x0F:
                                   break;
                             }
                           }
                           //初始化用户控制器
                           Task_Motor_Controller_Init(&g_stc_MotorController);
                           Task_Encoder_SavedCalibrationResult(&g_stc_MotorRunStatusObser,&g_stc_MotorCommunicator);
                       }
                       else if (g_stc_MotorRunStatusObser.Motor_Status == MOTOR_MODE){
                        if(g_stc_MotorCommunicator.can_commuicator.recv_candata[0]==0x01)//正转
                        {
                            g_stc_MotorController.target_mech_theta = (float)((int)(g_stc_MotorCommunicator.can_commuicator.recv_candata[1] << 8 | g_stc_MotorCommunicator.can_commuicator.recv_candata[2]))*ANGLE2RAD;
                            g_stc_MotorController.target_mech_rpm = (int)(g_stc_MotorCommunicator.can_commuicator.recv_candata[3] << 8 | g_stc_MotorCommunicator.can_commuicator.recv_candata[4]);
                            g_stc_MotorController.target_iq = (int)(g_stc_MotorCommunicator.can_commuicator.recv_candata[5] << 8 | g_stc_MotorCommunicator.can_commuicator.recv_candata[6]);
                        }
                        else if(g_stc_MotorCommunicator.can_commuicator.recv_candata[0]==0x00)//反转
                        {
                            g_stc_MotorController.target_mech_theta = (float)(0-(int)(g_stc_MotorCommunicator.can_commuicator.recv_candata[1] << 8 | g_stc_MotorCommunicator.can_commuicator.recv_candata[2]))*ANGLE2RAD;
                            g_stc_MotorController.target_mech_rpm = 0-(int)(g_stc_MotorCommunicator.can_commuicator.recv_candata[3] << 8 | g_stc_MotorCommunicator.can_commuicator.recv_candata[4]);
                            g_stc_MotorController.target_iq = 0-(int)(g_stc_MotorCommunicator.can_commuicator.recv_candata[5] << 8 | g_stc_MotorCommunicator.can_commuicator.recv_candata[6]);
                        }
                       }
            }
       }
        memset(g_stc_MotorCommunicator.can_commuicator.recv_candata, 0, sizeof(g_stc_MotorCommunicator.can_commuicator.recv_candata));
        CAN_clearInterruptStatus(CANFDB_BASE, CAN_FLAG_CLR_ALL);
}





/**********磁编码器标定算法********************************************************************************************/
//电机开环运动函数，主要用于标定编码器
void Task_Motor_OpenRun(xt_task_motor_RunstatusObser *motorobser, xt_task_motor_Controller *controller)
{
    Math_SinCos(motorobser->Motor_StdElecAngle, & motorobser->Motor_IdqObser.sin,  & motorobser->Motor_IdqObser.cos);
    motorobser->Motor_IdqRef.Iq = controller->target_iq;
    motorobser->Motor_IdqRef.Id = controller->target_id;
    motorobser->Motor_IdqRef.sin = motorobser->Motor_IdqObser.sin;
    motorobser->Motor_IdqRef.cos = motorobser->Motor_IdqObser.cos;
    Transf_InvPark(motorobser->Motor_IdqRef, &motorobser->Motor_IalphaBetaRef);
    //2.SVPWM
    /** PWM calculation, and generate switch pattern of SVPWM *********/
    motorobser->Motor_PWM_Controller.svpwm_caculate(motorobser->Motor_IalphaBetaRef.Ialpha,motorobser->Motor_IalphaBetaRef.Ibeta,motorobser->Motor_Current_Sensor.StdVbusObser,&motorobser->Motor_PWM_Controller.svpwm_caculator);
    motorobser->Motor_PWM_Controller.motor_pwm_caculate(&motorobser->Motor_PWM_Controller.svpwm_caculator,&motorobser->Motor_PWM_Controller);
    motorobser->Motor_PWM_Controller.motor_pwm_set(motorobser->Motor_PWM_Controller.u16Uon,motorobser->Motor_PWM_Controller.u16Von,motorobser->Motor_PWM_Controller.u16Won);
}

//编码器相序校准
void Task_Encoder_DirCalibration(xt_task_motor_RunstatusObser *motorobser, xt_task_motor_Controller *controller)
{
      printf("\n\r Checking ZeroOffset\n\r");//首先进行零点位置校准，这个零点主要是用于电机机械角度
      float theta_ref    = 0.0f;
      float me_cur_angle = 0.0f;
      float re_cur_angle = 0.0f;
      float me_cur_angle_last = 0.0f;
      float re_cur_angle_last = 0.0f;
      controller->target_iq = 0.0f;
      controller->target_id = CALIB_D_Cur;
      motorobser->Motor_StdElecAngle = 0.0f;
      for(int i =0;i<20000;i++)
      {
          Task_Motor_OpenRun(motorobser,controller);
          DEVICE_DELAY_US(100);
      }
      DEVICE_DELAY_US(10000);
      //读取电角度为0位置
      motorobser->Motor_Encoder.me_encoder.encoder_update(&motorobser->Motor_Encoder.me_encoder);
      motorobser->Motor_Encoder.re_encoder.encoder_update(&motorobser->Motor_Encoder.re_encoder);
      me_cur_angle_last = motorobser->Motor_Encoder.me_encoder.abs_multi_mech_theta;
      re_cur_angle_last = motorobser->Motor_Encoder.re_encoder.abs_multi_mech_theta;
      printf("\n\r Checking phase ordering\n\r");
      while(theta_ref<MATH_PI*0.25)
      {
          motorobser->Motor_StdElecAngle = normalizeAngle(theta_ref*MOTOR_PolePairs)*INV_MATH_TWO_PI;
          Task_Motor_OpenRun(motorobser,controller);
          DEVICE_DELAY_US(1000);
          motorobser->Motor_Encoder.me_encoder.encoder_update(&motorobser->Motor_Encoder.me_encoder);
          theta_ref += 0.0005f;
      }
      DEVICE_DELAY_US(10000);
      motorobser->Motor_Encoder.me_encoder.encoder_update(&motorobser->Motor_Encoder.me_encoder);
      motorobser->Motor_Encoder.re_encoder.encoder_update(&motorobser->Motor_Encoder.re_encoder);
      me_cur_angle = motorobser->Motor_Encoder.me_encoder.abs_multi_mech_theta;
      re_cur_angle = motorobser->Motor_Encoder.re_encoder.abs_multi_mech_theta;
        //计算
        if(me_cur_angle-me_cur_angle_last>0.01)
        {
            motorobser->Motor_Encoder.me_encoder.orderdir = 1;
        }
        else {
            motorobser->Motor_Encoder.me_encoder.orderdir = -1;
        }
        if(re_cur_angle-re_cur_angle_last>0.01)
        {
            motorobser->Motor_Encoder.re_encoder.orderdir = 1;
        }
        else {
            motorobser->Motor_Encoder.re_encoder.orderdir = -1;
        }
        while(theta_ref>0.0f)
          {
              motorobser->Motor_StdElecAngle = normalizeAngle(theta_ref*MOTOR_PolePairs)*INV_MATH_TWO_PI;
              Task_Motor_OpenRun(motorobser,controller);
              DEVICE_DELAY_US(1000);
              motorobser->Motor_Encoder.me_encoder.encoder_update(&motorobser->Motor_Encoder.me_encoder);
              theta_ref -= 0.0005f;
          }
        printf("\n\r Checking phase ordering Done! \n\r");
}

static float kahan_acc_aver_error_offset(float error_f[], float error_b[])
{
    float sum = 0.0f;
    float c = 0.0f;  // 补偿项

    for (int i = 0; i < CALIB_N; i++) {
        float value = (error_f[i] + error_b[CALIB_N - 1 - i]) / (2.0f * CALIB_N);
        float y = value - c;
        float t = sum + y;
        c = (t - sum) - y;
        sum = t;
    }
    return sum;
}

static void kahan_acc_fliter_error(float error[], float error_filt[])
{
    float c_filt[CALIB_N] = {0.0f};  // 每个 error_filt 元素的补偿项
    float c_mean = 0.0f;  // mean 的补偿项

    // 外层循环
    for (int i = 0; i < CALIB_N; i++) {
        float sum_filt = 0.0f;  // 当前 error_filt[i] 的累加和
        // 内层循环
        for (int j = 0; j < CALIB_WINDOW; j++) {
            int ind = -CALIB_WINDOW / 2 + j + i;
            if (ind < 0) {
                ind += CALIB_N;
            } else if (ind > CALIB_N - 1) {
                ind -= CALIB_N;
            }
            float value = error[ind] / (float)CALIB_WINDOW;
            float y = value - c_filt[i];
            float t = sum_filt + y;
            c_filt[i] = (t - sum_filt) - y;
            sum_filt = t;
        }
        error_filt[i] = sum_filt;
    }
}


static void kahan_acc_get_raw_value(xt_task_motor_RunstatusObser *motorobser, xt_task_motor_Controller *controller, float delta_step, float error_f[], int raw_f[], float error_b[], int raw_b[]) {
    float theta_ref = 0.0f;
    float c_theta_ref = 0.0f;  // theta_ref 的补偿项
    // 正向旋转部分
    for (int i = 0; i < CALIB_N; i++) {
        for (int j = 0; j < CALIB_NT; j++) {
            // Kahan 求和更新 theta_ref
            float y = delta_step - c_theta_ref;
            float t = theta_ref + y;
            c_theta_ref = (t - theta_ref) - y;
            theta_ref = t;
            motorobser->Motor_StdElecAngle = normalizeAngle(theta_ref) * INV_MATH_TWO_PI;
            Task_Motor_OpenRun(motorobser, controller);
            DEVICE_DELAY_US(100);
            motorobser->Motor_Encoder.me_encoder.encoder_update(&motorobser->Motor_Encoder.me_encoder);
        }
        motorobser->Motor_Encoder.me_encoder.encoder_update(&motorobser->Motor_Encoder.me_encoder);
        float theta_actual = motorobser->Motor_Encoder.me_encoder.abs_multi_mech_theta;
        error_f[i] = theta_ref / (float)(MOTOR_PolePairs) - theta_actual;
        raw_f[i] = motorobser->Motor_Encoder.me_encoder.encoder_base_info.raw_value;
        printf("%f,%f,%f\n",theta_ref/(float)(MOTOR_PolePairs),theta_actual,error_f[i]);
    }
    // 重置补偿项，可根据需求决定是否保留 theta_ref 原值
    c_theta_ref = 0.0f;
    // 反向旋转部分
    for (int i = 0; i < CALIB_N; i++) {
        for (int j = 0; j < CALIB_NT; j++) {
            // Kahan 求差更新 theta_ref
            float y = delta_step + c_theta_ref;
            float t = theta_ref - y;
            c_theta_ref = (t - theta_ref) + y;
            theta_ref = t;
            motorobser->Motor_StdElecAngle = normalizeAngle(theta_ref) * INV_MATH_TWO_PI;
            Task_Motor_OpenRun(motorobser, controller);
            DEVICE_DELAY_US(100);
            motorobser->Motor_Encoder.me_encoder.encoder_update(&motorobser->Motor_Encoder.me_encoder);
        }
        motorobser->Motor_Encoder.me_encoder.encoder_update(&motorobser->Motor_Encoder.me_encoder);
        float theta_actual = motorobser->Motor_Encoder.me_encoder.abs_multi_mech_theta;
        error_b[i] = theta_ref / (float)(MOTOR_PolePairs) - theta_actual;
        raw_b[i] = motorobser->Motor_Encoder.me_encoder.encoder_base_info.raw_value;
        printf("%f,%f,%f\n",theta_ref/(float)(MOTOR_PolePairs),theta_actual,error_b[i]);
    }
}


static float kahan_acc_sum_errorflit(float error_filt[])
{
    float sum = 0.0f;
    float c = 0.0f;  // 补偿项，用于记录精度损失
    for (int i = 0; i < CALIB_N; i++) {
        float y = error_filt[i] - c;
        float t = sum + y;
        c = (t - sum) - y;
        sum = t;
    }
    return sum;
}

//编码器零点校准和偏心率校准
void Task_Encoder_ZeroCalibration(xt_task_motor_RunstatusObser *motorobser, xt_task_motor_Controller *controller)
{
    float error_f[CALIB_N] = {0.0f};
     float error_b[CALIB_N] = {0.0f};
     int lut[LUT_N] = {0};
     int raw_f[CALIB_N] = {0};
     int raw_b[CALIB_N] = {0};
     float error[CALIB_N] = {0.0f};
     float error_filt[CALIB_N] = {0.0f};
     float mean = 0.0f;
     float delta_step =MATH_TWO_PI*(float)(MOTOR_PolePairs)/(float)(CALIB_N*CALIB_NT);
     float theta_ref = 0.0f; //[0-2PI]
     float theta_actual = 0.0f;//[0-2PI]
     float offset = 0.0f;
     printf("Starting encoder zero calibration procedure\n\r");
     //先找到一个就近的零点回正
     controller->target_iq = 0.0f;
     controller->target_id = CALIB_D_Cur;
     motorobser->Motor_StdElecAngle = 0.0f;
    for(int i = 0; i<40000; i++){
        Task_Motor_OpenRun(motorobser,controller);
        DEVICE_DELAY_US(300);
    }
    DEVICE_DELAY_US(10000);
    motorobser->Motor_Encoder.me_encoder.encoder_update(&motorobser->Motor_Encoder.me_encoder);
    motorobser->Motor_Encoder.re_encoder.encoder_update(&motorobser->Motor_Encoder.re_encoder);
    printf("zero value: %f,%f\n",motorobser->Motor_Encoder.me_encoder.abs_multi_mech_theta,motorobser->Motor_Encoder.re_encoder.abs_multi_mech_theta);
    motorobser->Motor_Encoder.me_encoder.encoder_set_mech_offset(motorobser->Motor_Encoder.me_encoder.abs_multi_mech_theta,&motorobser->Motor_Encoder.me_encoder);
    motorobser->Motor_Encoder.re_encoder.encoder_set_mech_offset(motorobser->Motor_Encoder.re_encoder.abs_multi_mech_theta,&motorobser->Motor_Encoder.re_encoder);
    DEVICE_DELAY_US(10000);
    kahan_acc_get_raw_value(motorobser,controller,delta_step, error_f,  raw_f,  error_b,  raw_b);
    offset = kahan_acc_aver_error_offset(error_f,error_b);
    offset = fmodf(offset*MOTOR_PolePairs, MATH_TWO_PI);                                        // convert mechanical angle to electrical angle
    motorobser->Motor_Encoder.me_encoder.encoder_set_elec_offset(offset,&motorobser->Motor_Encoder.me_encoder);
   for (int i = 0; i<CALIB_N; i++){                                              //Average the forward and back directions
              error[i] = 0.5f*(error_f[i] + error_b[CALIB_N-i-1]);
              }
   kahan_acc_fliter_error(error, error_filt);
   mean = kahan_acc_sum_errorflit(error_filt);
   mean = mean/(float)(CALIB_N);
   int raw_offset = (raw_f[0] + raw_b[CALIB_N-1])/2;                             //Insensitive to errors in this direction, so 2 points is plenty
   printf("\n\r Encoder non-linearity compensation table\n\r");
   printf(" Sample Number : Lookup Index : Lookup Value\n\r\n\r");
   for (int i = 0; i<LUT_N; i++){                                          // build lookup table
       int ind = (raw_offset>>7) + i;
       if(ind > (LUT_N-1)){
           ind -= LUT_N;
           }
       lut[ind] = (int) ((error_filt[i*MOTOR_PolePairs] - mean)*(float)(Motor_End_Encoder_Res)/MATH_TWO_PI);
       printf("%d   %d   %f  %d \n\r", i, ind,error_filt[i*MOTOR_PolePairs], lut[ind]);
       DEVICE_DELAY_US(1000);
       }
   memcpy(&motorobser->Motor_Encoder.me_encoder.offset_lut, lut, sizeof(lut));                                 // copy the lookup table to the flash array
   printf("\n\rEncoder Electrical Offset (rad) %f\n\r",  offset);
}

void Task_Encoder_SavedCalibrationResult(xt_task_motor_RunstatusObser *motorobser,xt_task_motor_Communicator *motorcommunitor)
{
   g_stc_LogFile.motor_can_id = motorcommunitor->Motor_CanID;
   g_stc_LogFile.master_can_id = motorcommunitor->Master_CanID;
   g_stc_LogFile.signflag = 0;
   if(motorobser->Motor_Encoder.me_encoder.elec_offset<0.0)
   {
       g_stc_LogFile.signflag|=0x0001;
       g_stc_LogFile.motor_elec_offset = (uint32_t)((0.0-motorobser->Motor_Encoder.me_encoder.elec_offset)*10000.0);
   }
   else
   {
       g_stc_LogFile.motor_elec_offset = (uint32_t)((motorobser->Motor_Encoder.me_encoder.elec_offset)*10000.0);
   }
   if(motorobser->Motor_Encoder.me_encoder.mech_theta_offset<0.0)
   {
       g_stc_LogFile.signflag|=0x0002;
       g_stc_LogFile.motor_mech_offset =  (uint32_t)((0.0-motorobser->Motor_Encoder.me_encoder.mech_theta_offset)*10000.0);
   }
   else
   {
       g_stc_LogFile.motor_mech_offset =  (uint32_t)((motorobser->Motor_Encoder.me_encoder.mech_theta_offset)*10000.0);
   }
   if(motorobser->Motor_Encoder.me_encoder.orderdir<0)
   {
       g_stc_LogFile.signflag|=0x0004;
       g_stc_LogFile.motor_orderdir = motorobser->Motor_Encoder.me_encoder.orderdir;
   }
   else {
       g_stc_LogFile.motor_orderdir = motorobser->Motor_Encoder.me_encoder.orderdir;
}

   if(motorobser->Motor_Encoder.re_encoder.mech_theta_offset<0.0)
   {
       g_stc_LogFile.signflag|=0x0008;
       g_stc_LogFile.reducer_mech_offset =  (uint32_t)((0.0-motorobser->Motor_Encoder.re_encoder.mech_theta_offset)*10000.0);
   }
   else
   {
       g_stc_LogFile.reducer_mech_offset =  (uint32_t)((motorobser->Motor_Encoder.re_encoder.mech_theta_offset)*10000.0);
   }
   if(motorobser->Motor_Encoder.re_encoder.orderdir<0)
   {
       g_stc_LogFile.signflag|=0x0010;
       g_stc_LogFile.reducer_orderdir = motorobser->Motor_Encoder.re_encoder.orderdir;
   }
   else {
       g_stc_LogFile.reducer_orderdir = motorobser->Motor_Encoder.re_encoder.orderdir;
}
   g_stc_LogFile.isUserDefFlag = 1;
   memcpy(&g_stc_LogFile.motor_offset_lut, motorobser->Motor_Encoder.me_encoder.offset_lut, sizeof(motorobser->Motor_Encoder.me_encoder.offset_lut));
   Hal_Write_Motor_Param(&g_stc_LogFile);
}

//编码器校准任务
void Task_Encoder_Calibration(xt_task_motor_RunstatusObser *motorobser, xt_task_motor_Controller *controller,xt_task_motor_Communicator *motorcommunitor)
{
    //复位校准参数
    motorobser->Motor_Encoder.me_encoder.elec_offset = 0.0f;
    motorobser->Motor_Encoder.me_encoder.mech_theta_offset = 0.0f;
    motorobser->Motor_Encoder.re_encoder.mech_theta_offset = 0.0f;
    for(int i=0;i<LUT_N;i++)
    {
        motorobser->Motor_Encoder.me_encoder.offset_lut[i] = 0;
    }
    //使能电机输出
    motorobser->Motor_PWM_Controller.motor_pwm_enable(TRUE);
   //编码器相序判断
   Task_Encoder_DirCalibration(motorobser,controller);
   DEVICE_DELAY_US(10000);
   //零点校准和偏心率校准
   Task_Encoder_ZeroCalibration(motorobser,controller);
   //保存校准值
   Task_Encoder_SavedCalibrationResult(motorobser,motorcommunitor);
   printf("\n\r Calibration complete.\n\r");
   //电机快速停止并初始化
   Task_Motor_FastStop(motorobser,controller);
   motorobser->Motor_Status_Change = 0;
}
